#include "mpc.h"
#include "iostream"
namespace LinearMpc
{
Mpc::Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr,
		 unsigned int mpc_windows,
		 unsigned int max_xu_constraint,
		 unsigned int max_delta_x,
		 unsigned int max_delta_u,
		 unsigned int max_final_constraint,
		 unsigned int max_xu_ae,
		 unsigned int max_xu_slack_ineq,
		 unsigned int max_x_final_ae,
		 unsigned int max_x_final_slack_ineq,
		 unsigned int max_delta_u_ae,
		 unsigned int max_delta_u_slack_ineq,
		 unsigned int max_delta_x_ae,
		 unsigned int max_delta_x_slack_ineq)
	: dis_sys_ptr_(dis_sys_ptr),
	  mpc_windows_(mpc_windows),
	  max_xu_constraint_(max_xu_constraint),
	  max_delta_x_(max_delta_x),
	  max_delta_u_(max_delta_u),
	  max_final_constraint_(max_final_constraint),

	  max_xu_ae_(max_xu_ae),
	  max_xu_slack_ineq_(max_xu_slack_ineq),
	  max_x_final_ae_(max_x_final_ae),
	  max_x_final_slack_ineq_(max_x_final_slack_ineq),
	  max_delta_u_ae_(max_delta_u_ae),
	  max_delta_u_slack_ineq_(max_delta_u_slack_ineq),
	  max_delta_x_ae_(max_delta_x_ae),
	  max_delta_x_slack_ineq_(max_delta_x_slack_ineq),
	  delta_u_u_index_(max_delta_u, 1000),
	  delta_u_min_(max_delta_u, 0),
	  delta_u_max_(max_delta_u, 0)
//1000 can be any value,big than max_delta_u
{

	state_dim_ = dis_sys_ptr->state_dim();
	control_dim_ = dis_sys_ptr->control_dim();
	distur_dim_ = dis_sys_ptr->distur_dim();
	x0_ = Eigen::VectorXd::Zero(state_dim_);
	//////////////////////
	u_minus1_ = Eigen::VectorXd::Zero(control_dim_); //std::vector<double>(control_dim_, 0);
	/////////////////////
	distur_ref_ = Eigen::VectorXd::Zero(distur_dim_);
	//dynamic x-ref is not supported now
	x_ref_ = Eigen::MatrixXd::Zero(state_dim_, 1);
	u_ref_ = Eigen::MatrixXd::Zero(control_dim_, 1);

	q_ = Eigen::MatrixXd::Identity(state_dim_, state_dim_);
	r_ = Eigen::MatrixXd::Zero(control_dim_, control_dim_);
	xu_ae_w_ = std::vector<double>(max_xu_ae_, 1);
	xu_slack_ineq_w_ = std::vector<double>(max_xu_slack_ineq_, 1);
	x_final_ae_w_ = std::vector<double>(max_x_final_ae_, 1);
	x_final_slack_ineq_w_ = std::vector<double>(max_x_final_slack_ineq_, 1);
	delta_u_ae_w_ = std::vector<double>(max_delta_u_ae_, 1);
	delta_u_slack_ineq_w_ = std::vector<double>(max_delta_u_slack_ineq_, 1);
	delta_x_ae_w_ = std::vector<double>(max_delta_x_ae_, 1);
	delta_x_slack_ineq_w_ = std::vector<double>(max_delta_x_slack_ineq, 1);

	u_1_row_start_index_ = state_dim_ * (mpc_windows_ + 1);
	xu_row_start_index_ = u_1_row_start_index_ + control_dim_;
	delta_x_row_start_index_ = xu_row_start_index_ + max_xu_constraint_ * mpc_windows_;
	//delta_u0_row_start_index_ = delta_x_row_start_index_ + max_delta_x_ * mpc_windows_;
	delta_u_row_start_index_ = delta_x_row_start_index_ + max_delta_x_ * mpc_windows_;
	final_row_start_index_ = delta_u_row_start_index_ + max_delta_u_ * mpc_windows_;

	xu_ae_row_start_index_ = final_row_start_index_ + max_final_constraint_;
	xu_slack_ineq_row_start_index_ = xu_ae_row_start_index_ + max_xu_ae_ * mpc_windows_;
	x_final_ae_row_start_index_ = xu_slack_ineq_row_start_index_ + 2 * max_xu_slack_ineq_ * mpc_windows_;
	x_final_slack_ineq_row_start_index_ = x_final_ae_row_start_index_ + max_x_final_ae_;

	delta_u_ae_row_start_index_ = x_final_slack_ineq_row_start_index_ + 2 * max_x_final_slack_ineq_;
	delta_u_slack_ineq_row_start_index_ = delta_u_ae_row_start_index_ + max_delta_u_ae_ * mpc_windows_;
	delta_x_ae_row_start_index_ = delta_u_slack_ineq_row_start_index_ + 2 * max_delta_u_slack_ineq_ * mpc_windows_;
	delta_x_slack_ineq_row_start_index_ = delta_x_ae_row_start_index_ + max_delta_x_ae_ * mpc_windows_;

	unsigned int xu_ineq_row_start_slack = delta_x_slack_ineq_row_start_index_ + 2 * max_delta_x_slack_ineq_ * mpc_windows_;
	unsigned int x_final_ineq_row_start_slack = xu_ineq_row_start_slack + max_xu_slack_ineq_ * mpc_windows_;
	unsigned int delta_u_row_ineq_slack = x_final_ineq_row_start_slack + max_x_final_slack_ineq_;
	unsigned int delta_x_row_ineq_slack = delta_u_row_ineq_slack + max_delta_u_slack_ineq * mpc_windows_;

	constriants_num_ = delta_x_row_ineq_slack + max_delta_x_slack_ineq_ * mpc_windows_;

	u_1_col_start_index_ = state_dim_ * (mpc_windows_ + 1);
	u_col_start_index_ = u_1_col_start_index_ + control_dim_;
	xu_ae_col_start_index_ = u_col_start_index_ + control_dim_ * mpc_windows_;
	xu_slack_ineq_col_start_index_ = xu_ae_col_start_index_ + max_xu_ae_ * mpc_windows_;
	x_final_ae_col_start_index_ = xu_slack_ineq_col_start_index_ + max_xu_slack_ineq_ * mpc_windows_;
	x_final_slack_ineq_col_start_index_ = x_final_ae_col_start_index_ + max_x_final_ae_;
	delta_u_ae_col_start_index_ = x_final_slack_ineq_col_start_index_ + max_x_final_slack_ineq_;
	delta_u_slack_ineq_col_start_index_ = delta_u_ae_col_start_index_ + max_delta_u_ae_ * mpc_windows_;
	delta_x_ae_col_start_index_ = delta_u_slack_ineq_col_start_index_ + max_delta_u_slack_ineq_ * mpc_windows_;
	delta_x_slack_ineq_col_start_index_ = delta_x_ae_col_start_index_ + max_delta_x_ae_ * mpc_windows_;

	opti_var_num_ = delta_x_slack_ineq_col_start_index_ + max_delta_x_slack_ineq * mpc_windows_;

	hessian_ = Eigen::SparseMatrix<double>(opti_var_num_, opti_var_num_);
	gradient_ = Eigen::VectorXd::Zero(opti_var_num_);

	constraint_matrix_ = Eigen::SparseMatrix<double>(constriants_num_, opti_var_num_);
	lowerBound_ = Eigen::VectorXd::Zero(constriants_num_);
	upperBound_ = Eigen::VectorXd::Zero(constriants_num_);
	//init state_fun eq identity part and u_mins_1 in constraint_matrix_
	for (int i = 0; i < state_dim_ * (mpc_windows_ + 1) + control_dim_; i++)
	{
		constraint_matrix_.insert(i, i) = -1;
	}
	//ineq slack variable 0<s<infinte

	for (unsigned int constriant_index = 0; constriant_index < max_xu_slack_ineq_; constriant_index++)
	{
		unsigned int start_row_index = xu_ineq_row_start_slack + constriant_index * mpc_windows_;
		unsigned int start_col_index = xu_slack_ineq_col_start_index_ + constriant_index * mpc_windows_;
		unsigned int temp_row_index;
		for (unsigned int time_index = 0; time_index < mpc_windows_; time_index++)
		{
			temp_row_index = start_row_index + time_index;
			constraint_matrix_.coeffRef(temp_row_index, start_col_index + time_index) = 1;
			lowerBound_(temp_row_index) = 0;
			upperBound_(temp_row_index) = OSQP_INFTY;
		}
	}
	for (unsigned int constriant_index = 0; constriant_index < max_x_final_slack_ineq_; constriant_index++)
	{
		unsigned int row_index = x_final_ineq_row_start_slack + constriant_index;
		unsigned int col_index = x_final_slack_ineq_col_start_index_ + constriant_index;
		constraint_matrix_.coeffRef(row_index, col_index) = 1;
		lowerBound_(row_index) = 0;
		upperBound_(row_index) = OSQP_INFTY;
	}

	for (unsigned int constriant_index = 0; constriant_index < max_delta_u_slack_ineq_; constriant_index++)
	{
		unsigned int start_row_index = delta_u_row_ineq_slack + constriant_index * mpc_windows_;
		unsigned int start_col_index = delta_u_slack_ineq_col_start_index_ + constriant_index * mpc_windows_;
		unsigned int temp_row_index;
		for (unsigned int time_index = 0; time_index < mpc_windows_; time_index++)
		{
			temp_row_index = start_row_index + time_index;
			constraint_matrix_.coeffRef(temp_row_index, start_col_index + time_index) = 1;
			lowerBound_(temp_row_index) = 0;
			upperBound_(temp_row_index) = OSQP_INFTY;
		}
	}
	for (unsigned int constriant_index = 0; constriant_index < max_delta_x_slack_ineq_; constriant_index++)
	{
		unsigned int start_row_index = delta_x_row_ineq_slack + constriant_index * mpc_windows_;
		unsigned int start_col_index = delta_x_slack_ineq_col_start_index_ + constriant_index * mpc_windows_;
		unsigned int temp_row_index;
		for (unsigned int time_index = 0; time_index < mpc_windows_; time_index++)
		{
			temp_row_index = start_row_index + time_index;
			constraint_matrix_.coeffRef(temp_row_index, start_col_index + time_index) = 1;
			lowerBound_(temp_row_index) = 0;
			upperBound_(temp_row_index) = OSQP_INFTY;
		}
	}
	set_hessian();
	set_gradient();
	set_sf_constraint();
	clear_changed_flag();
	// settings
	is_solver_inited_ = false;
	if (!init_osqp_solver())
		std::cerr << "\033[31m init_osqp_solver in constrcutor failed  \033[0m" << std::endl;
}
bool Mpc::init_osqp_solver()
{
	// settings
	solver_.settings()
		->setVerbosity(true);
	solver_.settings()->setWarmStart(true);
	solver_.settings()->setPolish(true);
	// set the initial data of the QP solver_
	solver_.data()->setNumberOfVariables(opti_var_num_);
	solver_.data()->setNumberOfConstraints(constriants_num_);

	solver_.data()->clearHessianMatrix();
	if (!solver_.data()->setHessianMatrix(hessian_))
		return false;
	solver_.data()->clearLinearConstraintsMatrix();
	if (!solver_.data()->setLinearConstraintsMatrix(constraint_matrix_))
		return false;
	if (!solver_.data()->setGradient(gradient_))
		return false;
	if (!solver_.data()->setLowerBound(lowerBound_))
		return false;
	if (!solver_.data()->setUpperBound(upperBound_))
		return false;
	if (!solver_.initSolver())
		return false;
	is_solver_inited_ = true;

	return true;
}

void Mpc::set_hessian()
{

	if (is_q_changed_)
	{
		for (int i = 0; i < state_dim_ * (mpc_windows_ + 1) + control_dim_ * mpc_windows_; i++)
		{
			if (i < state_dim_ * (mpc_windows_ + 1))
			{
				int posQ = i % state_dim_;
				double value = q_.diagonal()[posQ];
				hessian_.coeffRef(i, i) = value;
			}
		}
	}
	if (is_r_changed_)
	{

		for (unsigned int time_index = 0; time_index < mpc_windows_; time_index++)
		{
			unsigned int time_anchor_index = u_col_start_index_ + control_dim_ * time_index;
			for (unsigned int control_index = 0; control_index < control_dim_; control_index++)
			{
				unsigned int temp_index = time_anchor_index + control_index;
				hessian_.coeffRef(temp_index, temp_index) = r_(control_index, control_index);
			}
		}
	}
	if (is_xu_ae_w_changed_)
	{
		for (unsigned int constriants_index = 0; constriants_index < max_xu_ae_; constriants_index++)
		{
			unsigned int one_contraints_start_index = xu_ae_col_start_index_ + constriants_index * mpc_windows_;
			unsigned int temp_index;
			for (unsigned int time_index = 0; time_index < mpc_windows_; time_index++)
			{
				temp_index = one_contraints_start_index + time_index;
				hessian_.coeffRef(temp_index, temp_index) = xu_ae_w_[constriants_index];
			}
		}
	}
	if (is_xu_slack_ineq_w_changed_)
	{
		for (unsigned int constriants_index = 0; constriants_index < max_xu_slack_ineq_; constriants_index++)
		{
			unsigned int one_contraints_start_index = xu_slack_ineq_col_start_index_ + constriants_index * mpc_windows_;
			unsigned int temp_index;
			for (unsigned int time_index = 0; time_index < mpc_windows_; time_index++)
			{
				temp_index = one_contraints_start_index + time_index;
				hessian_.coeffRef(temp_index, temp_index) = xu_slack_ineq_w_[constriants_index];
			}
		}
	}
	if (is_x_final_ae_w_changed_)
	{

		for (unsigned int constriants_index = 0; constriants_index < max_x_final_ae_; constriants_index++)
		{
			unsigned int one_contraints_start_index = x_final_ae_col_start_index_ + constriants_index;
			hessian_.coeffRef(one_contraints_start_index, one_contraints_start_index) = x_final_ae_w_[constriants_index];
		}
	}
	if (is_x_final_slack_ineq_w_changed_)
	{
		for (unsigned int constriants_index = 0; constriants_index < max_x_final_slack_ineq_; constriants_index++)
		{
			unsigned int one_contraints_start_index = x_final_slack_ineq_col_start_index_ + constriants_index;
			hessian_.coeffRef(one_contraints_start_index, one_contraints_start_index) = x_final_slack_ineq_w_[constriants_index];
		}
	}
	if (is_delta_u_ae_w_changed_)
	{
		for (unsigned int constriants_index = 0; constriants_index < max_delta_u_ae_; constriants_index++)
		{
			unsigned int one_contraints_start_index = delta_u_ae_col_start_index_ + constriants_index * mpc_windows_;
			unsigned int temp_index;
			for (unsigned int time_index = 0; time_index < mpc_windows_; time_index++)
			{
				temp_index = one_contraints_start_index + time_index;
				hessian_.coeffRef(temp_index, temp_index) = delta_u_ae_w_[constriants_index];
			}
		}
	}
	if (is_delta_u_slack_ineq_w_changed_)
	{
		for (unsigned int constriants_index = 0; constriants_index < max_delta_u_slack_ineq_; constriants_index++)
		{
			unsigned int one_contraints_start_index = delta_u_slack_ineq_col_start_index_ + constriants_index * mpc_windows_;
			unsigned int temp_index;
			for (unsigned int time_index = 0; time_index < mpc_windows_; time_index++)
			{
				temp_index = one_contraints_start_index + time_index;
				hessian_.coeffRef(temp_index, temp_index) = delta_u_slack_ineq_w_[constriants_index];
			}
		}
	}
	if (is_delta_x_ae_w_changed_)
	{
		for (unsigned int constriants_index = 0; constriants_index < max_delta_x_ae_; constriants_index++)
		{
			unsigned int one_contraints_start_index = delta_x_ae_col_start_index_ + constriants_index * mpc_windows_;
			unsigned int temp_index;
			for (unsigned int time_index = 0; time_index < mpc_windows_; time_index++)
			{
				temp_index = one_contraints_start_index + time_index;
				hessian_.coeffRef(temp_index, temp_index) = delta_x_ae_w_[constriants_index];
			}
		}
	}
	if (is_delta_x_slack_ineq_w_changed_)
	{
		for (unsigned int constriants_index = 0; constriants_index < max_delta_x_slack_ineq_; constriants_index++)
		{
			unsigned int one_contraints_start_index = delta_x_slack_ineq_col_start_index_ + constriants_index * mpc_windows_;
			unsigned int temp_index;
			for (unsigned int time_index = 0; time_index < mpc_windows_; time_index++)
			{
				temp_index = one_contraints_start_index + time_index;
				hessian_.coeffRef(temp_index, temp_index) = delta_x_slack_ineq_w_[constriants_index];
			}
		}
	}
}
void Mpc::set_gradient()
{
	if (is_q_changed_ || is_x_ref_changed_)
	{
		if (x_ref_.cols() == 1)
		{
			std::cout << "##########" << q_ << std::endl;
			std::cout << "##########" << x_ref_ << std::endl;
			Eigen::MatrixXd Qx_ref = q_ * (-x_ref_); //nx x 1
			// populate the gradient vector
			for (int i = 0; i < mpc_windows_ + 1; i++)
			{
				//block(0, 0, state_dim_, 1) = -x0_;
				gradient_.block(i * state_dim_, 0, state_dim_, 1) = Qx_ref;
			}
		}
		else if (x_ref_.cols() == (mpc_windows_ + 1))
		{
			Eigen::MatrixXd Qx_ref = q_ * (-x_ref_); ////nx x (mpc_windows_ + 1)
			int col_index = 0;
			for (int i = 0; i < mpc_windows_ + 1; i++)
			{
				gradient_.block(i * state_dim_, 0, state_dim_, 1) = Qx_ref.block(0, i, state_dim_, 1);
			}
		}
	}

	if (is_r_changed_ || is_u_ref_changed_)
	{
		if (u_ref_.cols() == 1)
		{
			Eigen::MatrixXd Ru_ref = r_ * (-u_ref_); //nu x 1

			// populate the gradient vector
			for (int i = 0; i < mpc_windows_; i++)
			{
				//block(0, 0, state_dim_, 1) = -x0_;
				gradient_.block(u_col_start_index_ + i * control_dim_, 0, control_dim_, 1) = Ru_ref;
			}
		}

		else if (u_ref_.cols() == mpc_windows_)
		{
			Eigen::MatrixXd Ru_ref = r_ * (-u_ref_); ////nu x mpc_windows_
			int col_index = 0;
			for (int i = 0; i < mpc_windows_; i++)
			{
				gradient_.block(u_col_start_index_ + i * control_dim_, 0, control_dim_, 1) = Ru_ref.block(0, i, control_dim_, 1);
			}
		}
	}
}

void Mpc::set_sf_constraint()
{

	if (dis_sys_ptr_->is_control_matrix_changed_ && dis_sys_ptr_->is_state_matrix_changed_)
	{

		Eigen::MatrixXd state_matrix = dis_sys_ptr_->state_matrix();
		Eigen::MatrixXd control_matrix = dis_sys_ptr_->control_matrix();
		for (int h_i = 0; h_i < mpc_windows_; h_i++)
		{
			for (int row_j = 0; row_j < state_dim_; row_j++)
			{
				for (int col_k = 0; col_k < state_dim_; col_k++)
				{
					float value = state_matrix(row_j, col_k);
					constraint_matrix_.coeffRef(state_dim_ * (h_i + 1) + row_j, state_dim_ * h_i + col_k) = value;
				}
				for (int col_k = 0; col_k < control_dim_; col_k++)
				{
					float value = control_matrix(row_j, col_k);
					constraint_matrix_.coeffRef(
						state_dim_ * (h_i + 1) + row_j, control_dim_ * (h_i + 1) + col_k + state_dim_ * (mpc_windows_ + 1)) = value;
				}
			}
		}
	}
	else if (dis_sys_ptr_->is_control_matrix_changed_ && !dis_sys_ptr_->is_state_matrix_changed_)
	{
		Eigen::MatrixXd control_matrix = dis_sys_ptr_->control_matrix();
		for (int h_i = 0; h_i < mpc_windows_; h_i++)
		{
			for (int row_j = 0; row_j < state_dim_; row_j++)
			{

				for (int col_k = 0; col_k < control_dim_; col_k++)
				{
					float value = control_matrix(row_j, col_k);
					constraint_matrix_.coeffRef(
						state_dim_ * (h_i + 1) + row_j, control_dim_ * h_i + col_k + state_dim_ * (mpc_windows_ + 1)) = value;
				}
			}
		}
	}
	else if (!dis_sys_ptr_->is_control_matrix_changed_ && dis_sys_ptr_->is_state_matrix_changed_)
	{
		Eigen::MatrixXd state_matrix = dis_sys_ptr_->state_matrix();
		for (int h_i = 0; h_i < mpc_windows_; h_i++)
		{
			for (int row_j = 0; row_j < state_dim_; row_j++)
			{
				for (int col_k = 0; col_k < state_dim_; col_k++)
				{
					float value = state_matrix(row_j, col_k);
					constraint_matrix_.coeffRef(state_dim_ * (h_i + 1) + row_j, state_dim_ * h_i + col_k) = value;
				}
			}
		}
	}
	else
	{
	}

	lowerBound_.block(0, 0, state_dim_, 1) = -x0_;
	upperBound_.block(0, 0, state_dim_, 1) = -x0_;
	for (unsigned int ctr_index = 0; ctr_index < control_dim_; ctr_index++)
	{
		unsigned int row_index = u_1_row_start_index_ + ctr_index;
		lowerBound_(row_index) = -u_minus1_(ctr_index);
		upperBound_(row_index) = -u_minus1_(ctr_index);
	}
	if (distur_dim_ > 0)
	{
		if (is_distur_ref_changed_ || dis_sys_ptr_->is_distur_matrix_changed_)
		{
			if (distur_dim_ != 0)
			{
				if (distur_ref_.cols() == 1)
				{
					Eigen::MatrixXd temp = dis_sys_ptr_->distur_matrix() * distur_ref_ * (-1);
					for (int i = 0; i < mpc_windows_; i++)
					{
						lowerBound_.block((i + 1) * state_dim_, 0, state_dim_, 1) = temp;
						upperBound_.block((i + 1) * state_dim_, 0, state_dim_, 1) = temp;
					}
				}
				else if (distur_ref_.cols() == mpc_windows_)
				{
					Eigen::MatrixXd temp_matrix = dis_sys_ptr_->distur_matrix() * distur_ref_ * (-1); //nx *n_h
					for (int i = 0; i < mpc_windows_; i++)
					{

						lowerBound_.block((i + 1) * state_dim_, 0, state_dim_, 1) =
							temp_matrix.block(0, i, state_dim_, 1);
						upperBound_.block((i + 1) * state_dim_, 0, state_dim_, 1) =
							temp_matrix.block(0, i, state_dim_, 1);
					}
				}
			}
		}
	}
}

bool Mpc::set_q(const Eigen::MatrixXd &q)
{
#ifdef SAFE_CHEAK
	if (q.rows() != state_dim_)
	{
		std::cout << "\033[31m Error:Mpc::set_q failed,q dim is not equal to state_dim  \033[0m"
				  << std::endl;
		return false;
	}
#endif
	if (q_ != q)
	{
		q_ = q;
		is_q_changed_ = true;
	}
	return true;
}
bool Mpc::set_r(const Eigen::MatrixXd &r)
{
#ifdef SAFE_CHEAK
	if (r.rows() != control_dim_)
	{
		std::cout << "\033[31m Error:Mpc::set_r failed,r dim is not equal to control_dim  \033[0m"
				  << std::endl;
		return false;
	}
#endif
	if (r_ != r)
	{
		r_ = r;
		is_r_changed_ = true;
	}
	return true;
}
bool Mpc::set_xu_ae_w(const std::vector<double> &xu_ae_w)
{
#ifdef SAFE_CHEAK
	if (xu_ae_w.size() > max_xu_ae_)
	{
		std::cout << "\033[31m Error:Mpc::set_xu_ae_w faile,dxu_ae_w.size() > max_xu_ae_  \033[0m"
				  << std::endl;
		return false;
	}
#endif
	if (xu_ae_w_ != xu_ae_w)
	{
		xu_ae_w_ = xu_ae_w;
		is_xu_ae_w_changed_ = true;
	}
	return true;
}
bool Mpc::set_xu_slack_ineq_w(const std::vector<double> &xu_slack_ineq_w)
{
#ifdef SAFE_CHEAK
	if (xu_slack_ineq_w.size() != max_xu_slack_ineq_)
	{
		std::cout << "\033[31m Error:Mpc::set_xu_slack_ineq_w failed,xu_slack_ineq_w.rows() != max_xu_slack_ineq_  \033[0m"
				  << std::endl;
		return false;
	}
#endif
	if (xu_slack_ineq_w_ != xu_slack_ineq_w)
	{
		xu_slack_ineq_w_ = xu_slack_ineq_w;
		is_xu_slack_ineq_w_changed_ = true;
	}
	return true;
}
bool Mpc::set_x_final_ae_w(const std::vector<double> &x_final_ae_w)
{
#ifdef SAFE_CHEAK
	if (x_final_ae_w.size() > max_x_final_ae_)
	{
		std::cout << "\033[31m Error:Mpc::x_final_ae failed,x_final_ae_w.size() > max_x_final_ae_  \033[0m"
				  << std::endl;
		return false;
	}
#endif
	if (x_final_ae_w != x_final_ae_w_)
	{
		x_final_ae_w_ = x_final_ae_w;
		is_x_final_ae_w_changed_ = true;
	}
	return true;
}
bool Mpc::set_x_final_slack_ineq_w(const std::vector<double> &x_final_slack_ineq_w)
{
#ifdef SAFE_CHEAK
	if (x_final_slack_ineq_w.size() > max_x_final_slack_ineq_)
	{
		std::cout << "\033[31m Error:Mpc::x_final_slack_ineq_w failed,x_final_slack_ineq_w.rows() > max_x_final_slack_ineq  \033[0m"
				  << std::endl;
		return false;
	}
#endif

	if (x_final_slack_ineq_w != x_final_slack_ineq_w_)
	{
		x_final_slack_ineq_w_ = x_final_slack_ineq_w;
		is_x_final_slack_ineq_w_changed_ = true;
	}
	return true;
}
bool Mpc::set_delta_u_ae_w(const std::vector<double> &delta_u_ae_w)
{
#ifdef SAFE_CHEAK
	if (delta_u_ae_w.size() > max_delta_u_ae_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_u_ae_w failed, delta_u_ae_w.size() > max_delta_u_ae_  \033[0m"
				  << std::endl;
		return false;
	}
#endif
	if (delta_u_ae_w_ != delta_u_ae_w)
	{
		delta_u_ae_w_ = delta_u_ae_w;
		is_delta_u_ae_w_changed_ = true;
	}
	return true;
}
bool Mpc::set_delta_u_slack_ineq_w(const std::vector<double> &delta_u_slack_ineq_w)
{
#ifdef SAFE_CHEAK
	if (delta_u_slack_ineq_w.size() > max_delta_u_slack_ineq_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_u_slack_ineq_w failed, delta_u_slack_ineq_w.size() > max_delta_u_slack_ineq_  \033[0m"
				  << std::endl;
		return false;
	}
#endif
	if (delta_u_slack_ineq_w_ != delta_u_slack_ineq_w)
	{
		delta_u_slack_ineq_w_ = delta_u_slack_ineq_w;
		is_delta_u_slack_ineq_w_changed_ = true;
	}
	return true;
}
bool Mpc::set_delta_x_ae_w(const std::vector<double> &delta_x_ae_w)
{
#ifdef SAFE_CHEAK
	if (delta_x_ae_w.size() > max_delta_x_ae_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_x_ae_w failed, delta_x_ae_w.size() > max_delta_x_ae_  \033[0m"
				  << std::endl;
		return false;
	}
#endif
	if (delta_x_ae_w_ != delta_x_ae_w)
	{
		delta_x_ae_w_ = delta_x_ae_w;
		is_delta_x_ae_w_changed_ = true;
	}
	return true;
}
bool Mpc::set_delta_x_slack_ineq_w(const std::vector<double> &delta_x_slack_ineq_w)
{
#ifdef SAFE_CHEAK
	if (delta_x_slack_ineq_w.size() > max_delta_x_slack_ineq_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_x_slack_ineq_w failed, delta_x_slack_ineq_w.size() > max_delta_x_slack_ineq_  \033[0m"
				  << std::endl;
		return false;
	}
#endif
	if (delta_x_slack_ineq_w_ != delta_x_slack_ineq_w)
	{
		delta_x_slack_ineq_w_ = delta_x_slack_ineq_w;
		is_delta_x_slack_ineq_w_changed_ = true;
	}
	return true;
}
Eigen::MatrixXd Mpc::q()
{
	return q_;
}
Eigen::MatrixXd Mpc::r()
{
	return r_;
}
std::vector<double> Mpc::xu_ae_w()
{
	return xu_ae_w_;
}
std::vector<double> Mpc::xu_slack_ineq_w()
{
	return xu_slack_ineq_w_;
}
std::vector<double> Mpc::x_final_ae_w()
{
	return x_final_ae_w_;
}
std::vector<double> Mpc::x_final_slack_ineq_w()
{
	return x_final_slack_ineq_w_;
}
std::vector<double> Mpc::delta_u_ae_w()
{
	return delta_u_ae_w_;
}
std::vector<double> Mpc::delta_u_slack_ineq_w()
{
	return delta_u_slack_ineq_w_;
}
std::vector<double> Mpc::delta_x_ae_w()
{
	return delta_x_ae_w_;
}
std::vector<double> Mpc::delta_x_slack_ineq_w()
{
	return delta_x_slack_ineq_w_;
}

bool Mpc::set_x0(const Eigen::VectorXd &x0)
{
#ifdef SAFE_CHEAK
	if (x0.rows() != state_dim_)
	{
		std::cout << "\033[31m Error:set_constraint_vector failed,x0 dim is not (state_dim_,1) \033[0m"
				  << std::endl;
		return false;
	}
#endif
	x0_ = x0;
	return true;
}
bool Mpc::set_u_minus1(const Eigen::VectorXd &u_minus1)
{
#ifdef SAFE_CHEAK
	if (u_minus1.cols() != control_dim_)
	{
		std::cout << "\033[31m Error:set_u_minus1 failed,u_minus1.size() != control_dim_ \033[0m"
				  << std::endl;
		return false;
	}
#endif

	u_minus1_ = u_minus1;
	return true;
} // namespace LinearMpc
bool Mpc::set_distur_ref(const Eigen::MatrixXd &distur_ref)
{
#ifdef SAFE_CHEAK
	if ((distur_ref.cols() != 1) && (distur_ref.cols() != mpc_windows_) && (distur_ref.cols() != 0))
	{
		std::cout << "\033[31m Error:set_distur_ref failed,distur_ref cols dim is not 1 or mpc_windows_ \033[0m"
				  << std::endl;
		return false;
	}
	if ((distur_ref.rows() != distur_dim_) && (distur_ref.cols() != 0))
	{
		std::cout << "\033[31m Error:set_distur_ref failed,distur_ref row dim is not distur_dim_ \033[0m"
				  << std::endl;
		return false;
	}
#endif
	distur_ref_ = distur_ref;
	is_distur_ref_changed_ = true;
	return true;
}
bool Mpc::set_x_ref(const Eigen::MatrixXd &x_ref)
{
#ifdef SAFE_CHEAK
	if ((x_ref.cols() != 1) && (x_ref.cols() != mpc_windows_ + 1))
	{
		std::cout << "\033[31m Error:set_x_ref failed,x_ref cols dim is not 1 or mpc_windows_ \033[0m"
				  << std::endl;
		return false;
	}
	if (x_ref.rows() != state_dim_)
	{
		std::cout << "\033[31m Error:set_x_ref failed,x_ref row dim is not state_dim_ \033[0m"
				  << std::endl;
		return false;
	}
#endif
	x_ref_ = x_ref;
	is_x_ref_changed_ = true;
	return true;
}
bool Mpc::set_u_ref(const Eigen::MatrixXd &u_ref)
{
#ifdef SAFE_CHEAK
	if ((u_ref.cols() != 1) && (u_ref.cols() != mpc_windows_))
	{
		std::cout << "\033[31m Error:set_u_ref failed,u_ref cols dim is not 1 or mpc_windows_ \033[0m"
				  << std::endl;
		return false;
	}
	if (u_ref.rows() != control_dim_)
	{
		std::cout << "\033[31m Error:set_u_ref failed,u_ref row dim is not control_dim_ \033[0m"
				  << std::endl;
		return false;
	}
#endif
	u_ref_ = u_ref;
	is_u_ref_changed_ = true;
	return true;
}
Eigen::VectorXd Mpc::x0()
{
	return x0_;
}
Eigen::MatrixXd Mpc::distur_ref()
{
	return distur_ref_;
}
Eigen::MatrixXd Mpc::x_ref()
{
	return x_ref_;
}
Eigen::MatrixXd Mpc::u_ref()
{
	return u_ref_;
}
uint Mpc::state_dim()
{
	return state_dim_;
}
uint Mpc::control_dim()
{
	return control_dim_;
}
uint Mpc::distur_dim()
{
	return distur_dim_;
}
uint Mpc::mpc_windows()
{
	return mpc_windows_;
}
std::shared_ptr<DiscreteLinearSys> Mpc::dis_sys()
{
	return dis_sys_ptr_;
}

Eigen::VectorXd Mpc::get_optimal_ctr()
{
	return optimal_ctr_;
}
Eigen::SparseMatrix<double> Mpc::hessian()
{
	return hessian_;
}
Eigen::SparseMatrix<double> Mpc::constraint_matrix()
{
	return constraint_matrix_;
}

Eigen::VectorXd Mpc::gradient()
{
	return gradient_;
}
Eigen::VectorXd Mpc::lowerBound()
{
	return lowerBound_;
}
Eigen::VectorXd Mpc::upperBound()
{
	return upperBound_;
}
Eigen::VectorXd Mpc::qp_solution()
{
	return qp_solution_;
}
std::vector<double> Mpc::get_state_vector(int index)
{
	if ((index >= state_dim_) || (index < 0))
	{
		std::cout << "\033[31m Error:Mpc::get_state_vector failed state index out of range. \033[0m"
				  << std::endl;
	}

	Eigen::MatrixXd temp_qp_solution = qp_solution_.block(0, 0, state_dim_ * (mpc_windows_ + 1), 1);
	temp_qp_solution.resize(state_dim_, (mpc_windows_ + 1));

	Eigen::VectorXd temp_vector = temp_qp_solution.block(index, 0, 1, (mpc_windows_ + 1)).transpose();
	std::vector<double> state_std(temp_vector.data(), temp_vector.data() + temp_vector.rows() * temp_vector.cols());

	return state_std;
}
Eigen::VectorXd Mpc::get_state_one_shot(unsigned int time_index)
{
	if (time_index > mpc_windows_)
	{
		std::cout << "\033[31m Error:Mpc::get_state_one_shot failed time_index out of range. \033[0m"
				  << std::endl;
	}

	Eigen::MatrixXd temp_qp_solution = qp_solution_.block(time_index * state_dim_, 0, state_dim_, 1);
	return temp_qp_solution;
}
Eigen::VectorXd Mpc::get_contorl_one_shot(unsigned int time_index)
{
	if (time_index > mpc_windows_ - 1)
	{
		std::cout << "\033[31m Error:Mpc::get_contorl_one_shot failed time_index out of range. \033[0m"
				  << std::endl;
	}

	Eigen::MatrixXd temp_qp_solution = qp_solution_.block((mpc_windows_ + 1) * state_dim_ + (time_index + 1) * control_dim_,
														  0, control_dim_, 1);
	return temp_qp_solution;
}

std::vector<double> Mpc::get_control_vector(int index)
{
	if ((index >= control_dim_) || (index < 0))
	{
		std::cout << "\033[31m Error:Mpc::get_control_vector failed control index out of range. \033[0m"
				  << std::endl;
	}

	Eigen::MatrixXd temp_qp_solution = qp_solution_.block(state_dim_ * (mpc_windows_ + 1) + control_dim_, 0, control_dim_ * mpc_windows_, 1);
	temp_qp_solution.resize(control_dim_, mpc_windows_);

	Eigen::VectorXd temp_vector = temp_qp_solution.block(index, 0, 1, mpc_windows_).transpose();
	std::vector<double> control_std(temp_vector.data(), temp_vector.data() + temp_vector.rows() * temp_vector.cols());

	return control_std;
}
int Mpc::set_xu_affine(const std::vector<double> x_index, const std::vector<double> x_factor,
					   const std::vector<double> u_index, const std::vector<double> u_factor,
					   int constraint_index, double min, double max)
{
#ifdef SAFE_CHEAK
	if (constraint_index >= max_xu_constraint_)
	{
		std::cout << "\033[31m Error:Mpc::add_xu_affine failed: constraint_index >= max_xu_constraint_  \033[0m" << std::endl;
		return -1;
	}
	if (u_index.size() != u_factor.size())
	{
		std::cout << "\033[31m Error:Mpc::add_xu_affine failed: u_index.size() != u_factor.size()  \033[0m" << std::endl;
		return -1;
	}

	if (x_index.size() != x_factor.size())
	{
		std::cout << "\033[31m Error:Mpc::add_xu_affine failed: x_index.size() != x_factor.size()  \033[0m" << std::endl;
		return -1;
	}
	for (int i = 0; i < x_index.size(); i++) //one row , full state vector
	{

		if (x_index[i] > state_dim_ - 1)
		{
			std::cout << "\033[31m Error:Mpc::add_xu_affine failed state_dim out of range error  \033[0m" << std::endl;
			return -1;
		}
	}
	for (int i = 0; i < u_index.size(); i++) //one row , full state vector
	{

		if (u_index[i] > control_dim_ - 1)
		{
			std::cout << "\033[31m Error:Mpc::add_xu_affine  failed control_dim error \033[0m" << std::endl;
			return -1;
		}
	}
#endif
	unsigned int one_constriant_row_start_index = xu_row_start_index_ + constraint_index * mpc_windows_;
	unsigned int row_index;
	for (unsigned int h_i = 0; h_i < mpc_windows_; h_i++)
	{
		row_index = one_constriant_row_start_index + h_i;
		for (int i = 0; i < x_index.size(); i++) //one row , full state vector
		{
			constraint_matrix_.coeffRef(
				row_index,
				state_dim_ * h_i + x_index[i]) = x_factor[i];
		}
		for (int i = 0; i < u_index.size(); i++) //one row , full state vector
		{
			constraint_matrix_.coeffRef(
				row_index,
				u_col_start_index_ + control_dim_ * h_i + u_index[i]) = u_factor[i];
		}
		lowerBound_(row_index) = min;
		upperBound_(row_index) = max;
	}
	return constraint_index;
}
int Mpc::set_xu_bound(int constraint_index, double min, double max)
{
#ifdef SAFE_CHEAK
	if (constraint_index >= max_xu_constraint_)
	{
		std::cout << "\033[31m Error:Mpc::add_xu_affine failed: constraint_index >= max_xu_constraint_  \033[0m" << std::endl;
		return -1;
	}
#endif
	unsigned int one_constriant_row_start_index = xu_row_start_index_ + constraint_index * mpc_windows_;
	unsigned int row_index;
	for (unsigned int h_i = 0; h_i < mpc_windows_; h_i++)
	{
		row_index = one_constriant_row_start_index + constraint_index * mpc_windows_ + h_i;
		lowerBound_(row_index) = min;
		upperBound_(row_index) = max;
	}
	return constraint_index;
}
int Mpc::set_delta_x(unsigned int constraint_index, unsigned int x_index, double min, double max)
{
#ifdef SAFE_CHEAK
	if (x_index >= state_dim_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_x  failed,x_index>=state_dim_ \033[0m" << std::endl;
		return -1;
	}
	if (constraint_index >= max_delta_x_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_x  failed,constraint_index >= max_delta_x_ \033[0m" << std::endl;
		return -1;
	}
	if (min > max)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_x  failed,min > max_ \033[0m" << std::endl;
		return -1;
	}
#endif
	unsigned int one_constriant_row_start_index = delta_x_row_start_index_ + constraint_index * mpc_windows_;
	unsigned int row_index;
	unsigned int col_index;

	for (int h_i = 0; h_i < mpc_windows_; h_i++)
	{
		row_index = one_constriant_row_start_index + h_i;
		col_index = state_dim_ * h_i + x_index;
		constraint_matrix_.coeffRef(
			row_index,
			col_index) = -1;
		constraint_matrix_.coeffRef(
			row_index,
			col_index + state_dim_) = 1;
		lowerBound_(row_index) = min;
		upperBound_(row_index) = max;
	}
	return constraint_index;
}
int Mpc::set_delta_x_bound(unsigned int constraint_index, unsigned int x_index, double min, double max)
{
#ifdef SAFE_CHEAK
	if (x_index >= state_dim_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_x  failed,x_index>=state_dim_ \033[0m" << std::endl;
		return -1;
	}
	if (constraint_index >= max_delta_x_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_x  failed,constraint_index >= max_delta_x_ \033[0m" << std::endl;
		return -1;
	}
	if (min > max)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_x  failed,min > max_ \033[0m" << std::endl;
		return -1;
	}
#endif
	unsigned int one_constriant_row_start_index = delta_x_row_start_index_ + constraint_index * mpc_windows_;
	for (int h_i = 0; h_i < mpc_windows_; h_i++)
	{
		unsigned int row_index = one_constriant_row_start_index + h_i;
		lowerBound_(row_index) = min;
		upperBound_(row_index) = max;
	}
	return constraint_index;
}

int Mpc::set_delta_u(unsigned int constraint_index, unsigned int u_index, double min, double max)
{
#ifdef SAFE_CHEAK
	if (u_index >= control_dim_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_u  failed,u_index > = control_dim_ \033[0m" << std::endl;
		return -1;
	}
	if (constraint_index >= max_delta_u_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_u  failed,constraint_index >= max_delta_u \033[0m" << std::endl;
		return -1;
	}
	if (min > max)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_u  failed,min > max_ \033[0m" << std::endl;
		return -1;
	}
#endif

	for (unsigned int h_i = 0; h_i < mpc_windows_; h_i++)
	{
		unsigned int row_index = delta_u_row_start_index_ + constraint_index * mpc_windows_ + h_i;
		unsigned int col_index = u_1_col_start_index_ + control_dim_ * h_i + u_index;
		constraint_matrix_.coeffRef(
			row_index,
			col_index) = -1;
		constraint_matrix_.coeffRef(
			row_index,
			col_index + control_dim_) = 1;
		lowerBound_(row_index) = min;
		upperBound_(row_index) = max;
	}
	return constraint_index;
}

int Mpc::set_delta_u_bound(unsigned int constraint_index, unsigned int u_index, double min, double max)
{
#ifdef SAFE_CHEAK
	if (u_index >= control_dim_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_u  failed,u_index > = control_dim_ \033[0m" << std::endl;
		return -1;
	}
	if (constraint_index >= max_delta_u_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_u  failed,constraint_index >= max_delta_u_ \033[0m" << std::endl;
		return -1;
	}
	if (min > max)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_u  failed,min > max_ \033[0m" << std::endl;
		return -1;
	}
#endif
	for (unsigned int h_i = 0; h_i < mpc_windows_; h_i++)
	{
		unsigned int row_index = delta_u_row_start_index_ + constraint_index * mpc_windows_ + h_i;
		lowerBound_(row_index) = min;
		upperBound_(row_index) = max;
	}
	return constraint_index;
}
int Mpc::set_final_constraint(int constraint_index, const std::vector<double> x_index,
							  const std::vector<double> x_factor, double min, double max)
{
#ifdef SAFE_CHEAK
	if (x_index.size() != x_factor.size())
	{
		std::cout << "\033[31m Error:Mpc::add_final_constraint failed: x_index.size() != x_factor.size()  \033[0m" << std::endl;
		return -1;
	}
#endif
	int row_index = final_row_start_index_ + constraint_index;
	for (int i = 0; i < x_index.size(); i++) //one row , full state vector
	{
#ifdef SAFE_CHEAK
		if (x_index[i] > state_dim_ - 1)
		{
			std::cout << "\033[31m Error:Mpc::add_final_constraint failed state_dim error  \033[0m" << std::endl;
			return -1;
		}
#endif
		constraint_matrix_.coeffRef(
			row_index,
			state_dim_ * mpc_windows_ + x_index[i]) = x_factor[i];
	}
	lowerBound_(row_index) = min;
	upperBound_(row_index) = max;
	return constraint_index;
}

int Mpc::set_final_bound(int constraint_index, double min, double max)
{
	int row_index = final_row_start_index_ + constraint_index;
	lowerBound_(row_index) = min;
	upperBound_(row_index) = max;
	return 0;
}
int Mpc::set_xu_ae(const std::vector<double> x_index, const std::vector<double> x_factor,
				   const std::vector<double> u_index, const std::vector<double> u_factor,
				   int constraint_index, double offset)
{
#ifdef SAFE_CHEAK
	if (constraint_index >= max_xu_ae_)
	{
		std::cout << "\033[31m Error:Mpc::set_xu_ae failed: constraint_index >= max_xu_ae_  \033[0m" << std::endl;
		return -1;
	}
	if (u_index.size() != u_factor.size())
	{
		std::cout << "\033[31m Error:Mpc::set_xu_ae failed: u_index.size() != u_factor.size()  \033[0m" << std::endl;
		return -1;
	}

	if (x_index.size() != x_factor.size())
	{
		std::cout << "\033[31m Error:Mpc::set_xu_ae failed: x_index.size() != x_factor.size()  \033[0m" << std::endl;
		return -1;
	}
	for (int i = 0; i < x_index.size(); i++) //one row , full state vector
	{

		if (x_index[i] > state_dim_ - 1)
		{
			std::cout << "\033[31m Error:Mpc::set_xu_ae failed state_dim out of range error  \033[0m" << std::endl;
			return -1;
		}
	}
	for (int i = 0; i < u_index.size(); i++) //one row , full state vector
	{

		if (u_index[i] > control_dim_ - 1)
		{
			std::cout << "\033[31m Error:Mpc::set_xu_ae  failed control_dim error \033[0m" << std::endl;
			return -1;
		}
	}
#endif
	unsigned int start_row_index = xu_ae_row_start_index_ + constraint_index * mpc_windows_;
	unsigned int row_index;
	for (int h_i = 0; h_i < mpc_windows_; h_i++)
	{
		row_index = start_row_index + h_i;
		for (int i = 0; i < x_index.size(); i++) //one row , full state vector
		{
			constraint_matrix_.coeffRef(
				row_index,
				state_dim_ * h_i + x_index[i]) = x_factor[i];
		}
		for (int i = 0; i < u_index.size(); i++) //one row , full state vector
		{
			constraint_matrix_.coeffRef(
				row_index,
				u_col_start_index_ + control_dim_ * h_i + u_index[i]) = u_factor[i];
		}
		constraint_matrix_.coeffRef(
			row_index,
			xu_ae_col_start_index_ + h_i) = -1;
		lowerBound_(row_index) = -offset;
		upperBound_(row_index) = -offset;
	}
	return constraint_index;
}
bool Mpc::set_xu_ae_offset(int constraint_index, double offset)
{
#ifdef SAFE_CHEAK
	if (constraint_index >= max_xu_ae_)
	{
		std::cout << "\033[31m Error:Mpc::set_xu_ae_offset failed: constraint_index >= max_xu_ae_  \033[0m" << std::endl;
		return false;
	}
#endif
	unsigned int start_row_index = xu_ae_row_start_index_ + constraint_index * mpc_windows_;
	unsigned int row_index;
	for (int h_i = 0; h_i < mpc_windows_; h_i++)
	{
		row_index = start_row_index + h_i;
		lowerBound_(row_index) = -offset;
		upperBound_(row_index) = -offset;
	}
	return true;
}
int Mpc::set_xu_slack_ineq(const std::vector<double> x_index, const std::vector<double> x_factor,
						   const std::vector<double> u_index, const std::vector<double> u_factor,
						   int constraint_index, double min, double max)
{
#ifdef SAFE_CHEAK
	if (constraint_index >= max_xu_slack_ineq_)
	{
		std::cout << "\033[31m Error:Mpc::set_xu_slack_ineq failed: constraint_index >= max_xu_slack_ineq_ \033[0m" << std::endl;
		return -1;
	}
	if (u_index.size() != u_factor.size())
	{
		std::cout << "\033[31m Error:Mpc::set_xu_slack_ineq failed: u_index.size() != u_factor.size()  \033[0m" << std::endl;
		return -1;
	}

	if (x_index.size() != x_factor.size())
	{
		std::cout << "\033[31m Error:Mpc::set_xu_slack_ineq failed: x_index.size() != x_factor.size()  \033[0m" << std::endl;
		return -1;
	}
	for (int i = 0; i < x_index.size(); i++) //one row , full state vector
	{

		if (x_index[i] > state_dim_ - 1)
		{
			std::cout << "\033[31m Error:Mpc::set_xu_slack_ineq failed state_dim out of range error  \033[0m" << std::endl;
			return -1;
		}
	}
	for (int i = 0; i < u_index.size(); i++) //one row , full state vector
	{

		if (u_index[i] > control_dim_ - 1)
		{
			std::cout << "\033[31m Error:Mpc::set_xu_slack_ineq  failed control_dim error \033[0m" << std::endl;
			return -1;
		}
	}
#endif
	unsigned int start_row_index = xu_slack_ineq_row_start_index_ + constraint_index * 2 * mpc_windows_;
	unsigned int row_index;
	for (int h_i = 0; h_i < mpc_windows_; h_i++)
	{
		row_index = start_row_index + 2 * h_i;
		for (int i = 0; i < x_index.size(); i++) //one row , full state vector
		{
			constraint_matrix_.coeffRef(
				row_index,
				state_dim_ * h_i + x_index[i]) = x_factor[i];
			constraint_matrix_.coeffRef(
				row_index + 1,
				state_dim_ * h_i + x_index[i]) = x_factor[i];
		}
		for (int i = 0; i < u_index.size(); i++) //one row , full state vector
		{
			constraint_matrix_.coeffRef(
				row_index,
				u_col_start_index_ + control_dim_ * h_i + u_index[i]) = u_factor[i];
			constraint_matrix_.coeffRef(
				row_index + 1,
				u_col_start_index_ + control_dim_ * h_i + u_index[i]) = u_factor[i];
		}
		constraint_matrix_.coeffRef(
			row_index,
			xu_slack_ineq_col_start_index_ + h_i) = 1;
		constraint_matrix_.coeffRef(
			row_index + 1,
			xu_slack_ineq_col_start_index_ + h_i) = -1;
		lowerBound_(row_index) = min;
		upperBound_(row_index) = OSQP_INFTY;
		lowerBound_(row_index + 1) = -OSQP_INFTY;
		upperBound_(row_index + 1) = max;
	}
	return constraint_index;
}
int Mpc::set_xu_slack_bound(int constraint_index, double min, double max)
{
#ifdef SAFE_CHEAK
	if (constraint_index >= max_xu_slack_ineq_)
	{
		std::cout << "\033[31m Error:Mpc::set_xu_slack_ineq failed: constraint_index >= max_xu_slack_ineq_ \033[0m" << std::endl;
		return -1;
	}
#endif
	unsigned int start_row_index = xu_slack_ineq_row_start_index_ + constraint_index * 2 * mpc_windows_;
	unsigned int row_index;
	for (int h_i = 0; h_i < mpc_windows_; h_i++)
	{
		row_index = start_row_index + 2 * h_i;
		constraint_matrix_.coeffRef(
			row_index,
			xu_slack_ineq_col_start_index_ + h_i) = 1;
		constraint_matrix_.coeffRef(
			row_index + 1,
			xu_slack_ineq_col_start_index_ + h_i) = -1;
		lowerBound_(row_index) = min;
		upperBound_(row_index) = OSQP_INFTY;
		lowerBound_(row_index + 1) = -OSQP_INFTY;
		upperBound_(row_index + 1) = max;
	}
	return constraint_index;
}
int Mpc::set_x_final_ae(int constraint_index, const std::vector<double> x_index,
						const std::vector<double> x_factor, double offset)
{
#ifdef SAFE_CHEAK
	if (constraint_index >= max_x_final_ae_)
	{
		std::cout << "\033[31m Error:Mpc::set_x_final_ae_offset failed ,constraint_index >= max_x_final_ae_ \033[0m" << std::endl;
		return -1;
	}
	if (x_index.size() != x_factor.size())
	{
		std::cout << "\033[31m Error:Mpc::set_x_final_ae failed: x_index.size() != x_factor.size()  \033[0m" << std::endl;
		return -1;
	}
#endif
	unsigned int row_index = x_final_ae_row_start_index_ + constraint_index;
	unsigned int col_start_index = state_dim_ * mpc_windows_;
	for (int i = 0; i < x_index.size(); i++) //one row , full state vector
	{
#ifdef SAFE_CHEAK
		if (x_index[i] > state_dim_ - 1)
		{
			std::cout << "\033[31m Error:Mpc::set_x_final_ae failed state_dim error  \033[0m" << std::endl;
			return -1;
		}
#endif
		constraint_matrix_.coeffRef(
			row_index,
			col_start_index + x_index[i]) = x_factor[i];
	}
	constraint_matrix_.coeffRef(
		row_index,
		x_final_ae_col_start_index_ + constraint_index) = -1;
	lowerBound_(row_index) = -offset;
	upperBound_(row_index) = -offset;
	return constraint_index;
}
int Mpc::set_x_final_ae_offset(int constraint_index, double offset)
{
#ifdef SAFE_CHEAK
	if (constraint_index >= max_x_final_ae_)
	{
		std::cout << "\033[31m Error:Mpc::set_x_final_ae_offset failed ,constraint_index >= max_x_final_ae_ \033[0m" << std::endl;
		return -1;
	}
#endif
	unsigned int row_index = x_final_ae_row_start_index_ + constraint_index;
	lowerBound_(row_index) = -offset;
	upperBound_(row_index) = -offset;
	return constraint_index;
}
int Mpc::set_x_final_slack_ineq(int constraint_index, const std::vector<double> x_index,
								const std::vector<double> x_factor, double min, double max)
{
#ifdef SAFE_CHEAK
	if (constraint_index >= max_x_final_slack_ineq_)
	{
		std::cout << "\033[31m Error:Mpc::set_x_final_slack_ineq failed ,constraint_index >= max_x_final_slack_ineq_ \033[0m" << std::endl;
		return -1;
	}
	if (x_index.size() != x_factor.size())
	{
		std::cout << "\033[31m Error:Mpc::set_x_final_slack_ineq failed: x_index.size() != x_factor.size()  \033[0m" << std::endl;
		return -1;
	}
#endif
	unsigned int row_index = x_final_slack_ineq_row_start_index_ + 2 * constraint_index;
	unsigned int col_start_index = state_dim_ * mpc_windows_;
	for (int i = 0; i < x_index.size(); i++) //one row , full state vector
	{
#ifdef SAFE_CHEAK
		if (x_index[i] > state_dim_ - 1)
		{
			std::cout << "\033[31m Error:Mpc::set_x_final_ae failed state_dim error  \033[0m" << std::endl;
			return -1;
		}
#endif
		constraint_matrix_.coeffRef(
			row_index,
			col_start_index + x_index[i]) = x_factor[i];
		constraint_matrix_.coeffRef(
			row_index + 1,
			col_start_index + x_index[i]) = x_factor[i];
	}
	constraint_matrix_.coeffRef(
		row_index,
		x_final_slack_ineq_col_start_index_ + constraint_index) = 1;
	constraint_matrix_.coeffRef(
		row_index + 1,
		x_final_slack_ineq_col_start_index_ + constraint_index) = -1;
	lowerBound_(row_index) = min;
	upperBound_(row_index) = OSQP_INFTY;
	lowerBound_(row_index + 1) = -OSQP_INFTY;
	upperBound_(row_index + 1) = max;
	return constraint_index;
}
int Mpc::set_x_final_slack_ineq_bound(int constraint_index, double min, double max)
{
#ifdef SAFE_CHEAK
	if (constraint_index >= max_x_final_slack_ineq_)
	{
		std::cout << "\033[31m Error:Mpc::set_x_final_slack_ineq failed ,constraint_index >= max_x_final_slack_ineq_ \033[0m" << std::endl;
		return -1;
	}

#endif
	unsigned int row_index = x_final_slack_ineq_row_start_index_ + 2 * constraint_index;
	lowerBound_(row_index) = min;
	upperBound_(row_index) = OSQP_INFTY;
	lowerBound_(row_index + 1) = -OSQP_INFTY;
	upperBound_(row_index + 1) = max;
	return constraint_index;
}
int Mpc::set_delta_u_ae(unsigned int constraint_index, unsigned int u_index, double offset)
{
#ifdef SAFE_CHEAK
	if (constraint_index >= max_delta_u_ae_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_u_ae failed ,constraint_index >= max_delta_u_ae_ \033[0m" << std::endl;
		return -1;
	}
	if (u_index >= control_dim_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_u_ae failed ,constraint_index >= control_dim_ \033[0m" << std::endl;
		return -1;
	}
#endif
	unsigned int start_row_index = delta_u_ae_row_start_index_ + constraint_index * mpc_windows_;
	unsigned int row_index;
	unsigned int u_col_index;
	unsigned int start_slack_col_index = delta_u_ae_col_start_index_ + constraint_index * mpc_windows_;
	unsigned int slack_col_index;
	for (unsigned int h_i = 0; h_i < mpc_windows_; h_i++)
	{
		row_index = start_row_index + h_i;
		u_col_index = u_1_col_start_index_ + control_dim_ * h_i + u_index;
		slack_col_index = start_slack_col_index + h_i;
		constraint_matrix_.coeffRef(
			row_index,
			u_col_index) = -1;
		constraint_matrix_.coeffRef(
			row_index,
			u_col_index + control_dim_) = 1;
		constraint_matrix_.coeffRef(
			row_index,
			slack_col_index) = -1;
		lowerBound_(row_index) = -offset;
		upperBound_(row_index) = -offset;
	}
	return constraint_index;
}
int Mpc::set_delta_u_ae_offset(unsigned int constraint_index, double offset)
{
#ifdef SAFE_CHEAK
	if (constraint_index >= max_delta_u_ae_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_u_ae_offset failed ,constraint_index >= max_delta_u_ae_ \033[0m" << std::endl;
		return -1;
	}

#endif
	unsigned int start_row_index = delta_u_ae_row_start_index_ + constraint_index * mpc_windows_;
	unsigned int row_index;
	for (unsigned int h_i = 0; h_i < mpc_windows_; h_i++)
	{
		row_index = start_row_index + h_i;
		lowerBound_(row_index) = -offset;
		upperBound_(row_index) = -offset;
	}
	return constraint_index;
}
int Mpc::set_delta_u_slack_ineq(unsigned int constraint_index, unsigned int u_index, double min, double max)
{
#ifdef SAFE_CHEAK
	if (constraint_index >= max_delta_u_slack_ineq_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_u_slack_ineq failed ,constraint_index >= max_delta_u_slack_ineq_ \033[0m" << std::endl;
		return -1;
	}
	if (u_index >= control_dim_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_u_slack_ineq failed ,constraint_index >= control_dim_ \033[0m" << std::endl;
		return -1;
	}
#endif
	unsigned int start_row_index = delta_u_slack_ineq_row_start_index_ + 2 * constraint_index * mpc_windows_;
	unsigned int row_index;
	unsigned int u_col_index;
	unsigned int start_slack_col_index = delta_u_slack_ineq_col_start_index_ + constraint_index * mpc_windows_;
	unsigned int slack_col_index;
	for (unsigned int h_i = 0; h_i < mpc_windows_; h_i++)
	{
		row_index = start_row_index + 2 * h_i;
		u_col_index = u_1_col_start_index_ + control_dim_ * h_i + u_index;
		slack_col_index = start_slack_col_index + h_i;
		constraint_matrix_.coeffRef(
			row_index,
			u_col_index) = -1;
		constraint_matrix_.coeffRef(
			row_index,
			u_col_index + control_dim_) = 1;
		constraint_matrix_.coeffRef(
			row_index,
			slack_col_index) = 1;
		lowerBound_(row_index) = min;
		upperBound_(row_index) = OSQP_INFTY;

		constraint_matrix_.coeffRef(
			row_index + 1,
			u_col_index) = -1;
		constraint_matrix_.coeffRef(
			row_index + 1,
			u_col_index + control_dim_) = 1;
		constraint_matrix_.coeffRef(
			row_index + 1,
			slack_col_index) = -1;
		lowerBound_(row_index + 1) = -OSQP_INFTY;
		upperBound_(row_index + 1) = max;
	}
	return constraint_index;
}
int Mpc::set_delta_u_slack_ineq_bound(unsigned int constraint_index, double min, double max)
{
#ifdef SAFE_CHEAK
	if (constraint_index >= max_delta_u_slack_ineq_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_u_slack_ineq_bound failed ,constraint_index >= max_delta_u_slack_ineq_ \033[0m" << std::endl;
		return -1;
	}
#endif
	unsigned int start_row_index = delta_u_slack_ineq_row_start_index_ + 2 * constraint_index * mpc_windows_;
	unsigned int row_index;
	for (unsigned int h_i = 0; h_i < mpc_windows_; h_i++)
	{
		row_index = start_row_index + 2 * h_i;
		lowerBound_(row_index) = min;
		upperBound_(row_index) = OSQP_INFTY;
		lowerBound_(row_index + 1) = -OSQP_INFTY;
		upperBound_(row_index + 1) = max;
	}
	return constraint_index;
}
int Mpc::set_delta_x_ae(unsigned int constraint_index, unsigned int x_index, double offset)
{
#ifdef SAFE_CHEAK
	if (constraint_index >= max_delta_x_ae_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_x_ae failed ,constraint_index >= max_delta_x_ae_ \033[0m" << std::endl;
		return -1;
	}
	if (x_index >= state_dim_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_x_ae failed ,x_index >= state_dim_ \033[0m" << std::endl;
		return -1;
	}
#endif
	unsigned int start_row_index = delta_x_ae_row_start_index_ + constraint_index * mpc_windows_;
	unsigned int row_index;
	unsigned int x_col_index;
	unsigned int start_slack_col_index = delta_x_ae_col_start_index_ + constraint_index * mpc_windows_;
	unsigned int slack_col_index;
	for (unsigned int h_i = 0; h_i < mpc_windows_; h_i++)
	{
		row_index = start_row_index + h_i;
		x_col_index = state_dim_ * h_i + x_index;
		slack_col_index = start_slack_col_index + h_i;
		constraint_matrix_.coeffRef(
			row_index,
			x_col_index) = -1;
		constraint_matrix_.coeffRef(
			row_index,
			x_col_index + state_dim_) = 1;
		constraint_matrix_.coeffRef(
			row_index,
			slack_col_index) = -1;
		lowerBound_(row_index) = -offset;
		upperBound_(row_index) = -offset;
	}
	return constraint_index;
}
int Mpc::set_delta_x_ae_offset(unsigned int constraint_index, double offset)
{
#ifdef SAFE_CHEAK
	if (constraint_index >= max_delta_x_ae_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_x_ae failed ,constraint_index >= max_delta_x_ae_ \033[0m" << std::endl;
		return -1;
	}
#endif
	unsigned int start_row_index = delta_x_ae_row_start_index_ + constraint_index * mpc_windows_;
	unsigned int row_index;
	for (unsigned int h_i = 0; h_i < mpc_windows_; h_i++)
	{
		row_index = start_row_index + h_i;
		lowerBound_(row_index) = -offset;
		upperBound_(row_index) = -offset;
	}
	return constraint_index;
}
int Mpc::set_delta_x_slack_ineq(unsigned int constraint_index, unsigned int x_index, double min, double max)
{
#ifdef SAFE_CHEAK
	if (constraint_index >= max_delta_x_slack_ineq_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_x_slack_ineq failed ,constraint_index >= max_delta_x_slack_ineq_ \033[0m" << std::endl;
		return -1;
	}
	if (x_index >= state_dim_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_x_slack_ineq failed ,constraint_index >= state_dim_ \033[0m" << std::endl;
		return -1;
	}
#endif
	unsigned int start_row_index = delta_x_slack_ineq_row_start_index_ + 2 * constraint_index * mpc_windows_;
	unsigned int row_index;
	unsigned int x_col_index;
	unsigned int start_slack_col_index = delta_x_slack_ineq_col_start_index_ + constraint_index * mpc_windows_;
	unsigned int slack_col_index;
	for (unsigned int h_i = 0; h_i < mpc_windows_; h_i++)
	{
		row_index = start_row_index + 2 * h_i;
		x_col_index = state_dim_ * h_i + x_index;
		slack_col_index = start_slack_col_index + h_i;
		constraint_matrix_.coeffRef(
			row_index,
			x_col_index) = -1;
		constraint_matrix_.coeffRef(
			row_index,
			x_col_index + state_dim_) = 1;
		constraint_matrix_.coeffRef(
			row_index,
			slack_col_index) = 1;
		lowerBound_(row_index) = min;
		upperBound_(row_index) = OSQP_INFTY;

		constraint_matrix_.coeffRef(
			row_index + 1,
			x_col_index) = -1;
		constraint_matrix_.coeffRef(
			row_index + 1,
			x_col_index + state_dim_) = 1;
		constraint_matrix_.coeffRef(
			row_index + 1,
			slack_col_index) = -1;
		lowerBound_(row_index + 1) = -OSQP_INFTY;
		upperBound_(row_index + 1) = max;
	}
	return constraint_index;
}
int Mpc::set_delta_x_slack_ineq_bound(unsigned int constraint_index, double min, double max)
{
#ifdef SAFE_CHEAK
	if (constraint_index >= max_delta_x_slack_ineq_)
	{
		std::cout << "\033[31m Error:Mpc::set_delta_x_slack_ineq_bound failed ,constraint_index >= max_delta_x_slack_ineq_ \033[0m" << std::endl;
		return -1;
	}

#endif
	unsigned int start_row_index = delta_x_slack_ineq_row_start_index_ + 2 * constraint_index * mpc_windows_;
	unsigned int row_index;
	for (unsigned int h_i = 0; h_i < mpc_windows_; h_i++)
	{
		row_index = start_row_index + 2 * h_i;
		lowerBound_(row_index) = min;
		upperBound_(row_index) = OSQP_INFTY;
		lowerBound_(row_index + 1) = -OSQP_INFTY;
		upperBound_(row_index + 1) = max;
	}
	return constraint_index;
}
void Mpc::clear_changed_flag()
{

	is_distur_ref_changed_ = false;
	is_x_ref_changed_ = false;
	is_u_ref_changed_ = false;
	is_q_changed_ = false;
	is_r_changed_ = false;

	is_xu_ae_w_changed_ = false;
	is_xu_slack_ineq_w_changed_ = false;
	is_x_final_ae_w_changed_ = false;
	is_x_final_slack_ineq_w_changed_ = false;
	is_delta_u_ae_w_changed_ = false;
	is_delta_u_slack_ineq_w_changed_ = false;
	is_delta_x_ae_w_changed_ = false;
	is_delta_x_slack_ineq_w_changed_ = false;

	dis_sys_ptr_->is_state_matrix_changed_ = false;
	dis_sys_ptr_->is_control_matrix_changed_ = false;
	dis_sys_ptr_->is_distur_matrix_changed_ = false;
}
bool Mpc::update_mpc(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
					 const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
					 const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, const std::vector<double> &xu_ae_w,
					 const std::vector<double> &xu_slack_ineq_w, const std::vector<double> &x_final_ae_w,
					 const std::vector<double> &x_final_slack_ineq_w, const std::vector<double> &delta_u_ae_w,
					 const std::vector<double> &delta_u_slack_ineq_w, const std::vector<double> &delta_x_ae_w,
					 const std::vector<double> &delta_x_slack_ineq_w)
{

	if (!set_x0(x0))
	{
		std::cout << "\033[31m Error:Mpc::step failed ,!!set_x0(x0) \033[0m" << std::endl;
		return false;
	}
	if (u_fb.cols() != 0)
	{
		if (!set_u_minus1(u_fb))
		{
			std::cout << "\033[31m Error:Mpc::step failed ,!set_u_minus1(u_fb) \033[0m" << std::endl;
			return false;
		}
	}
	if (!set_x_ref(x_ref))
	{
		std::cout << "\033[31m Error:Mpc::step failed ,!set_x_ref(x_ref) \033[0m" << std::endl;
		return false;
	}
	if (!set_u_ref(u_ref))
	{
		std::cout << "\033[31m Error:Mpc::step failed ,!set_u_ref(u_ref) \033[0m" << std::endl;
		return false;
	}
	if (!set_distur_ref(distur_ref))
	{
		std::cout << "\033[31m Error:Mpc::step failed ,!set_distur_ref(distur_ref) \033[0m" << std::endl;
		return false;
	}
	if (q.cols() != 0)
	{
		if (!set_q(q))
		{
			std::cout << "\033[31m Error:Mpc::step failed ,!set_q(q) \033[0m" << std::endl;
			return false;
		}
	}
	if (r.cols() != 0)
	{
		if (!set_r(r))
		{
			std::cout << "\033[31m Error:Mpc::step failed ,!set_r(r) \033[0m" << std::endl;
			return false;
		}
	}
	if (xu_ae_w.size() != 0)
	{
		if (!set_xu_ae_w(xu_ae_w))
		{
			std::cout << "\033[31m Error:Mpc::step failed ,!set_xu_ae_w(xu_ae_w) \033[0m" << std::endl;
			return false;
		}
	}
	if (xu_slack_ineq_w.size() != 0)
	{
		if (!set_xu_slack_ineq_w(xu_slack_ineq_w))
		{
			std::cout << "\033[31m Error:Mpc::step failed ,!set_xu_slack_ineq_w(xu_slack_ineq_w) \033[0m" << std::endl;
			return false;
		}
	}
	if (x_final_ae_w.size() != 0)
	{
		if (!set_x_final_ae_w(x_final_ae_w))
		{
			std::cout << "\033[31m Error:Mpc::step failed ,!set_x_final_ae_w(x_final_ae_w)  \033[0m" << std::endl;
			return false;
		}
	}
	if (x_final_slack_ineq_w.size() != 0)
	{
		if (!set_x_final_slack_ineq_w(x_final_slack_ineq_w))
		{
			std::cout << "\033[31m Error:Mpc::step failed ,!set_final_slack_ineq_w(x_final_slack_ineq_w)  \033[0m" << std::endl;
			return false;
		}
	}
	if (delta_u_ae_w.size() != 0)
	{
		if (!set_delta_u_ae_w(delta_u_ae_w))
		{
			std::cout << "\033[31m Error:Mpc::step failed ,!set_delta_u_ae_w(delta_u_ae_w)  \033[0m" << std::endl;
			return false;
		}
	}
	if (delta_u_slack_ineq_w.size() != 0)
	{
		if (!set_delta_u_slack_ineq_w(delta_u_slack_ineq_w))
		{
			std::cout << "\033[31m Error:Mpc::step failed ,!set_delta_u_slack_ineq_w(delta_u_slack_ineq_w)  \033[0m" << std::endl;
			return false;
		}
	}
	if (delta_x_ae_w.size() != 0)
	{
		if (!set_delta_x_ae_w(delta_x_ae_w))
		{
			std::cout << "\033[31m Error:Mpc::step failed ,!set_delta_x_ae_w(delta_x_ae_w)  \033[0m" << std::endl;
			return false;
		}
	}
	if (delta_x_slack_ineq_w.size() != 0)
	{
		if (!set_delta_x_slack_ineq_w(delta_x_slack_ineq_w))
		{
			std::cout << "\033[31m Error:Mpc::step failed ,!set_delta_x_slack_ineq_w(delta_x_slack_ineq_w)  \033[0m" << std::endl;
			return false;
		}
	}
	set_hessian();
	set_gradient();

	set_sf_constraint();
	clear_changed_flag();
	if (!solver_.updateHessianMatrix(hessian_))
		return false;

	if (!solver_.updateLinearConstraintsMatrix(constraint_matrix_))
		return false;
	if (!solver_.updateGradient(gradient_))
		return false;
	if (!solver_.updateBounds(lowerBound_, upperBound_))
		return false;
	return true;
}

bool Mpc::update_mpc(
	const Eigen::MatrixXd &q, const Eigen::MatrixXd &r,
	const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb, const Eigen::MatrixXd &x_ref,
	const Eigen::MatrixXd &distur_ref)
{
	if (!set_u_minus1(u_fb))
	{
		return false;
	}
	//update_delta_u0_bound();
	if (!update_mpc(q, r, x0, x_ref, distur_ref))
		return false;
	return true;
}
bool Mpc::update_mpc(
	const Eigen::MatrixXd &q, const Eigen::MatrixXd &r,
	const Eigen::VectorXd &x0, const Eigen::MatrixXd &x_ref,
	const Eigen::MatrixXd &distur_ref) // dis_sys_ptr with distur and without delta_u
{

	if (!set_q(q))
		return false;
	if (!set_r(r))
		return false;
	if (!set_x0(x0))
		return false;
	if (!set_x_ref(x_ref))
		return false;
	if (!set_distur_ref(distur_ref))
		return false;
	set_hessian();
	set_gradient();

	set_sf_constraint();
	clear_changed_flag();
	if (!solver_.updateHessianMatrix(hessian_))
		return false;

	if (!solver_.updateLinearConstraintsMatrix(constraint_matrix_))
		return false;
	if (!solver_.updateGradient(gradient_))
		return false;
	if (!solver_.updateBounds(lowerBound_, upperBound_))
		return false;
	return true;
}
bool Mpc::update_mpc(
	const Eigen::MatrixXd &q, const Eigen::MatrixXd &r,
	const Eigen::VectorXd &x0, const Eigen::MatrixXd &x_ref) // dis_sys_ptr without distur and without delta_u
{
	if (!set_q(q))
		return false;
	if (!set_r(r))
		return false;
	if (!set_x0(x0))
		return false;
	if (!set_x_ref(x_ref))
		return false;
	set_hessian();
	set_gradient();
	set_sf_constraint();
	clear_changed_flag();
	if (!solver_.updateHessianMatrix(hessian_))
		return false;

	if (!solver_.updateLinearConstraintsMatrix(constraint_matrix_))
		return false;
	if (!solver_.updateGradient(gradient_))
		return false;
	if (!solver_.updateBounds(lowerBound_, upperBound_))
		return false;
	return true;
}
bool Mpc::mpc_solve()
{
	if (!solver_.solve())
	{
		std::cout << "\033[31m Error:Mpc::solver_.solve() failed. \033[0m" << std::endl;
		return false;
	}
	else
	{
		qp_solution_ = solver_.getSolution();
		optimal_ctr_ = qp_solution_.block(state_dim_ * (mpc_windows_ + 1), 0, control_dim_, 1);
	}
	return true;
}
Eigen::VectorXd Mpc::optimal_ctr()
{
	return optimal_ctr_;
}
} // namespace LinearMpc